using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using UnityEngine;

public class AStar
{
    List<StarNode> m_OpenNodes = new List<StarNode>();
    List<StarNode> m_ClosedNodes = new List<StarNode>();
    StarNode m_CurrentNode;
    StarNode m_Destination;
    public StarNode[,] m_TotalNodes;
    Vector3 m_StartingPos;
    float m_Offset;
    int layersToIgnore;

    float TileDepth;
    float TileWidth;
    float AreaWidth;
    float AreaDepth;
    int GridCols;
    int GridRows;

    float m_MinHeight = 0;
    /// <summary>
    /// Crea los nodos para hacer el pathfinding
    /// </summary>
    /// <param name="startingPos">Posicion de donde se empiezan a crear los nodos(tiene que ser la esquina superior izquierda del area)</param>
    /// <param name="witdh">Ancho del area</param>
    /// <param name="depth">Largo del area</param>
    /// <param name="rayHeight">Altura desde donde tiro el rayo</param>
    /// <param name="minHeight">Diferencia minima de altura que tiene que haber para que los nodos sean adyacentes</param>
    /// <param name="row">Filas (en cuantos nodos dividis el largo)</param>
    /// <param name="col">Columnas(en cuantos nodos dividis el ancho)</param>
    /// <param name="offset">Altura para tirar rayos entre los nodos para saber si son adyacentes</param>
    public void CreateNodes(Vector3 startingPos, float witdh, float depth, float rayHeight,float minHeight, int row, int col, float offset)
    {
        m_TotalNodes = new StarNode[row, col];
        this.m_StartingPos = startingPos;
        this.m_Offset = offset;

        AreaWidth = witdh;
        AreaDepth = depth;
        TileDepth = depth / row;
        TileWidth = witdh / col;

        this.m_MinHeight = minHeight;
        this.GridCols = col;
        this.GridRows = row;

        layersToIgnore = 1 << 8;//character
        layersToIgnore += 1 << 2; //ignore de unity
        layersToIgnore += 1 << 9; // items
        layersToIgnore += 1 << 11; //guiitems
        layersToIgnore += 1 << 14;//enemy;
        for (int i = 0; i < row; i++)
        {
            for (int j = 0; j < col; j++)
            {
                Vector3 rayStart = GetPosFromCoord(i,j,rayHeight); //lo tiro desde arriba de todo al rayo
                Ray ray = new Ray(rayStart, Vector3.down);

                RaycastHit hitinfo;
                if(Physics.Raycast(ray,out hitinfo,rayHeight *1.2f,~(layersToIgnore)))
                {
                    if (hitinfo.transform.gameObject.layer != LayerMask.NameToLayer("Unreachable"))
                    {
                        StarNode node = new StarNode(hitinfo.point);
                        m_TotalNodes[i, j] = node;
                    }
                    else
                    {
                        m_TotalNodes[i, j] = null;
                    }
                }
            }
        }
        AddAdyacentsToGrid();
    }
    void AddNodesfromRaycastHits(RaycastHit[] hits, float minHeightDistance)
    {
        RaycastHit aux;
        //los ordeno por mayor altura
        for (int i = 0; i < hits.Length; i++)
        {
            for (int j = 0; j < hits.Length - i; j++)
            {
                if ((j < hits.Length - 1) && hits[j].point.y < hits[j+1].point.y)
                {
                    aux = hits[j];
                    hits[j] = hits[j + 1];
                    hits[j + 1] = aux;

                }
            }
        }

        CreateNode(hits[0].point);

        aux = hits[0];//ahora aux lo uso para el hit del ultimo nodo creado.
        for (int i = 1; i < hits.Length; i++)
        {
            if (Vector3.Distance(hits[i].point, aux.point) > aux.transform.localScale.y + minHeightDistance)
            {
                CreateNode(hits[i].point);
            }
            aux = hits[i];
        }
    }
    private void CreateNode(Vector3 pos)
    {
        StarNode node = new StarNode(pos);
        int nodeCol = -1;
        int nodeRow = -1;
        GetCoordFromPos(ref nodeRow, ref nodeCol, pos);
        m_TotalNodes[nodeRow, nodeCol] = node;
    }
    public void GetCoordFromPos(ref int row, ref int col, Vector3 pos)
    {
        col = (int)((pos.x - m_StartingPos.x) / TileWidth);
        row = (int)(-(pos.z - m_StartingPos.z) / TileDepth);

    }
    public Vector3 GetPosFromCoord(int row, int col, float height)
    {
        return new Vector3(m_StartingPos.x + col * TileWidth + TileWidth / 2,
                           height,
                           m_StartingPos.z - row * TileDepth - TileDepth / 2);
    }

    public StarNode GetNode(int row, int col)
    {
        if (row < 0) return null;
        if (col < 0) return null;
        if (row >= GridRows) return null;
        if (col >= GridCols) return null;
        return m_TotalNodes[row, col];
    }
    public StarNode GetNodefromPos(Vector3 pos)
    {
        int row = -1;
        int col = -1;
        GetCoordFromPos(ref row, ref col, pos);
        return GetNode(row, col);
    }
    public void LimpiarListas()
    {
        foreach (StarNode n in m_OpenNodes)
        {
            n.Reset();
        }
        foreach (StarNode n in m_ClosedNodes)
        {
            n.Reset();
        }
        m_OpenNodes.Clear();
        m_ClosedNodes.Clear();
    }

    /// <summary>
    /// le agrega a todos los nodos sus adyacentes
    /// </summary>
    void AddAdyacentsToGrid()
    {
        for (int i = 0; i < GridRows; i++)
        {
            for (int j = 0; j < GridCols; j++)
            {
                if (GetNode(i, j) == null) continue;
                AddAdyacentToNode(m_TotalNodes[i, j], i, j);
            }
        }
    }
    /// <summary>
    /// Le agrega los adyacentes a un nodo.
    /// </summary>
    /// <param name="node"></param>
    /// <param name="row">Fila del nodo.</param>
    /// <param name="col">Columna del nodo.</param>
    void AddAdyacentToNode(StarNode node,int row, int col)
    {
        for (int i = -1; i <= 1; i++)
        {
            for(int j = -1; j <= 1; j++)
            {
                if (i == 0 && j == 0) continue;
                StarNode nodeToAdd = GetNode(row + i, col + j);
                if (nodeToAdd != null)
                {
                    Ray ray = new Ray(node.Position + Vector3.up * m_Offset, nodeToAdd.Position - node.Position);
                    //Si el rayo no pega contra nada en el medio son adyacentes.
                    if (Physics.Raycast(ray, (nodeToAdd.Position - node.Position).magnitude + 1, ~(layersToIgnore)) == false)
                    {
                        if (Math.Abs(node.Position.y - nodeToAdd.Position.y) < m_MinHeight)
                        {
                            node.AddAdyacent(nodeToAdd);
                        }
                    }
                }
            }
        }
    }


    /// <summary>
    /// abre un nodo desde cierto padre
    /// </summary>
    /// <param name="node">Nodo a abrir</param>
    /// <param name="parent">Nodo que puede ser el padre del nodo a abrir</param>
    public void OpenNode(StarNode node, StarNode parent)
    {
        if (node.Closed) return;
        if (!node.Open)
        {
            m_OpenNodes.Add(node);
            node.Open = true;
        }
        if (parent != null)
        {
            node.SetParent(m_CurrentNode);
        }
    }
    /// <summary>
    /// Cierra un nodo y lo agrega a la lista de cerrados.
    /// </summary>
    /// <param name="node">Nodo a cerrar.</param>
    public void CloseNode(StarNode node)
    {
        m_OpenNodes.Remove(node);
        m_ClosedNodes.Add(node);
        node.Open = false;
        node.Closed = true;
    }
    /// <summary>
    /// abre los adyacentes de un nodo.
    /// </summary>
    /// <param name="node"></param>
    public void OpenAdyacents(StarNode node)
    {
        foreach (StarNode n in node.Adyacentes)
        {
            OpenNode(n, node);
        }
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="StartingNode">Nodo de partida</param>
    /// <param name="heightOffset">Altura que se le agrega a las posiciones del resultado</param>
    /// <returns>Una lista con el camino a recorrer, null si no hay camino disponible</returns>
    private List<Vector3> CalculatePath(StarNode StartingNode, float heightOffset)
    {
        OpenNode(StartingNode, null);

        List<Vector3> Path = new List<Vector3>();

        while (m_OpenNodes.Count > 0)
        {
            m_CurrentNode = SelectCurrentNode();
            if (m_CurrentNode == null)
            {
                Debug.Log("Current node is null");
                return null;
            }
            if (m_CurrentNode == m_Destination)
            {
                //si llego agrega las posiciones a una lista y lo devuelve dado vuelta.
                StarNode aux = m_CurrentNode;
                while (aux.GetParent() != null) // recorre hasta que el padre sea null, es decir hasta llegar al primer nodo del recorrido
                {
                    Path.Add(aux.Position + Vector3.up*heightOffset);
                    aux = aux.GetParent();
                }
                Path.Reverse(); //da vuelta la lista ya que se genera alrevez.
                LimpiarListas();
                return Path;
            }
            else
            {
                CloseNode(m_CurrentNode);
                OpenAdyacents(m_CurrentNode);
            }
        }
        LimpiarListas();
        return null;
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="origin">Posicion de origen.</param>
    /// <param name="destination">Posicion destino.</param>
    /// <param name="heightOffset">Altura que se le agrega a los vectores del resultado.</param>
    /// <returns>Una lista con el camino a recorrer, null si no hay camino disponible.</returns>
    public List<Vector3> GetPath(Vector3 origin, Vector3 destination, float heightOffset)
    {
        this.m_Destination = GetNodefromPos(destination);
        if (m_Destination == null)
        {
            Debug.Log("Destination is null");
            return null;
        }
        StarNode StartingNode = GetNodefromPos(origin);
        if (StartingNode == null)
        {
            Debug.Log("Starting node is null");
            GetNodefromPos(origin);
            return null;
        }
        return CalculatePath(StartingNode,heightOffset);
    }
    /// <summary>
    /// Elije el nodo que optimo usando el que menos score+heuristica tenga
    /// </summary>
    /// <returns></returns>
    public StarNode SelectCurrentNode()
    {
        StarNode OptimalNode = null;
        foreach (StarNode node in m_OpenNodes)
        {
            if (OptimalNode == null)
            {
                OptimalNode = node;
            }
            if (node.GetSH(m_Destination) < OptimalNode.GetSH(m_Destination))
            {
                OptimalNode = node;
            }
        }
        return OptimalNode;
    }
}